{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Predict state\n",
    "\n",
    "Here is the current implementation of the `predict_state` function. It takes in a state (a Python list), and then separates those into position and velocity to calculate a new, predicted state. It uses a constant velocity motion model.\n",
    "\n",
    "**In this exercise, we'll be improving this function, and using matrix multiplication to efficiently calculate the predicted state!**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "# The current predict state function\n",
    "# Predicts the next state based on a motion model\n",
    "def predict_state(state, dt):\n",
    "    # Assumes a valid state had been passed in\n",
    "    x = state[0]\n",
    "    velocity = state[1]\n",
    "    \n",
    "    # Assumes a constant velocity model\n",
    "    new_x = x + velocity*dt\n",
    "    \n",
    "    # Create and return the new, predicted state\n",
    "    predicted_state = [new_x, velocity]\n",
    "    return predicted_state\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Matrix operations\n",
    "\n",
    "You've been given a matrix class that can create new matrices and performs one operation: multiplication. In our directory this is called `matrix.py`.\n",
    "\n",
    "Similar to the Car class, we can use this to initialize matrix objects."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# import the matrix file\n",
    "import matrix\n",
    "\n",
    "# Initialize a state vector\n",
    "initial_position = 0 # meters\n",
    "velocity = 50 # m/s\n",
    "\n",
    "# Notice the syntax for creating a state column vector ([ [x], [v] ])\n",
    "# Commas separate these items into rows and brackets into columns\n",
    "initial_state = matrix.Matrix([ [initial_position], \n",
    "                                [velocity] ])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Transformation matrix\n",
    "\n",
    "Next, define the state transformation matrix and print it out!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[1 1 ]\n",
      "[0 1 ]\n",
      "]\n"
     ]
    }
   ],
   "source": [
    "# Define the state transformation matrix\n",
    "dt = 1\n",
    "tx_matrix = matrix.Matrix([ [1, dt], \n",
    "                            [0, 1] ])\n",
    "\n",
    "print(tx_matrix)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### TODO: Modify the predict state function to use matrix multiplication\n",
    "\n",
    "Now that you know how to create matrices, modify the `predict_state` function to work with them!\n",
    "\n",
    "Note: you can multiply a matrix A by a matrix B by writing `A*B` and it will return a new matrix.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "# The current predict state function\n",
    "def predict_state_mtx(state, dt):\n",
    "    ## TODO: Assume that the state passed in is a Matrix object\n",
    "    ## Using a constant velocity model and a transformation matrix\n",
    "    ## Create and return the new, predicted state!\n",
    "    tx_matrix = matrix.Matrix([ [1, dt], \n",
    "                            [0, 1] ])\n",
    "    predicted_state = tx_matrix*state\n",
    "    \n",
    "    return predicted_state"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Test cell\n",
    "\n",
    "Here is an initial state vector and dt to test your function with!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The initial state is: [[10 ]\n",
      "[30 ]\n",
      "]\n",
      "State after 2 seconds is: [[70.0 ]\n",
      "[30.0 ]\n",
      "]\n"
     ]
    }
   ],
   "source": [
    "# initial state variables\n",
    "initial_position = 10 # meters\n",
    "velocity = 30 # m/s\n",
    "\n",
    "# Initial state vector\n",
    "initial_state = matrix.Matrix([ [initial_position], \n",
    "                                [velocity] ])\n",
    "\n",
    "\n",
    "print('The initial state is: ' + str(initial_state))\n",
    "\n",
    "\n",
    "# after 2 seconds make a prediction using the new function\n",
    "state_est1 = predict_state_mtx(initial_state, 2)\n",
    "\n",
    "print('State after 2 seconds is: ' + str(state_est1))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "State after 3 more seconds is: [[160.0 ]\n",
      "[30.0 ]\n",
      "]\n",
      "Final state after 3 more seconds is: [[250.0 ]\n",
      "[30.0 ]\n",
      "]\n"
     ]
    }
   ],
   "source": [
    "# Make more predictions!\n",
    "\n",
    "# after 3 more\n",
    "state_est2 = predict_state_mtx(state_est1, 3)\n",
    "\n",
    "print('State after 3 more seconds is: ' + str(state_est2))\n",
    "\n",
    "# after 3 more\n",
    "state_est3 = predict_state_mtx(state_est2, 3)\n",
    "\n",
    "print('Final state after 3 more seconds is: ' + str(state_est3))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "anaconda-cloud": {},
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
